#include "AvcLcd.h"

AvcLcd::AvcLcd (AvcUtmPath* avcpath): lcd() {
  // set up the LCD's number of rows and columns: 
  lcd.begin();
  lcd.beginKeys();
  lcd.noBacklight();
  time = 0;
  init = true;
  backlit = false;
  mode = NONE;
  previousMode = NONE;
  waypointIndex = 0;
  path = avcpath;
}

void AvcLcd::setMode (Mode m) {
  previousMode = mode;
  mode = m;
  init = true;
  if (mode == NONE) {
    lcd.clear();
  }
}

void AvcLcd::display () {
  switch(mode) {
    case SAMPLING:
      sampling();
      break;
    case WAYPOINTS:
      waypointSlideshow();
      break;
  }
}

void AvcLcd::sampling () {
  if (init) {
    lcd.clear();
    lcd.print(F("SAMPLING"));
    init = false;
    time = millis();
    lcd.backlight();
    backlit = true;
  }
  if ((millis() - time) > 200) {
    if (backlit) {
      lcd.noBacklight();
      backlit = false;
    } else {
      lcd.backlight();
      backlit = true;
    }
    time = millis();
  } 
}

void AvcLcd::resetMode () {
  mode = previousMode;
  display();
}

void AvcLcd::printGps (long lat, long lon, float hdop, boolean refresh, int nextWaypoint) {
  if (refresh) {
    lcd.clear();
  }
  lcd.home();
  lcd << _FLOAT(lat/1000000.0, 6);
  lcd.setCursor(0, 1);
  lcd << _FLOAT(lon/1000000.0, 6);
  lcd.setCursor(12, 0);
  lcd << _FLOAT(hdop, 2);
  lcd.setCursor(12, 1);
  lcd << nextWaypoint << "  ";
  lcd.backlight();
}

void AvcLcd::printStartSampling(byte waypoints) {
  lcd.clear();
  lcd.home();
  lcd << F("START SAMPLING?");
  lcd.setCursor(0, 1);
  lcd << F("WAYPOINTS: ") << waypoints;
  lcd.backlight();
}

void AvcLcd::askReset(byte waypoints) {
  lcd.clear();
  lcd.home();
  lcd << F("RESET WAYPOINTS?");
  lcd.setCursor(0, 1);
  lcd << F("WAYPOINTS: ") << waypoints;
  lcd.backlight();
}

void AvcLcd::askWaypointSlideshow(byte waypoints) {
  lcd.clear();
  lcd << F("SHOW WAYPOINTS?");
  lcd.setCursor(0, 1);
  lcd << F("WAYPOINTS: ") << waypoints;
  lcd.backlight();
}

void AvcLcd::waypointSlideshow () {
  if (init) {
    lcd.clear();
    init = false;
    time = millis();
    waypointIndex = 0;
    lcd.backlight();
  }
  if ((millis() - time) > 1000) {
    byte count = path->getNumberOfWaypointsSet();
	//TODO
    long lat = path->getLatitudeForDisplay(waypointIndex);
    long lon = path->getLongitudeForDisplay(waypointIndex);
    lcd.home();
    lcd << _FLOAT(lat/1000000.0, 6);
    lcd.setCursor(0, 1);
    lcd << _FLOAT(lon/1000000.0, 6);
    lcd.setCursor(12, 0);
    lcd << waypointIndex;
    time = millis();
    waypointIndex = (waypointIndex + 1) % count;
  } 
}

void AvcLcd::askSetMaxSpeed(float maxSpeed, boolean refresh) {
  if (refresh) {
    lcd.clear();
    lcd.home();
    lcd << F("SET MAX SPEED?");
  }
  lcd.setCursor(0, 1);
  float n = getPotSpeed(1);
  lcd << F("C ") << _FLOAT(maxSpeed, 3) << F(" ");
  if (n > 0)
    lcd << F("N ") << _FLOAT(n, 3)  << F("  ");
  lcd.backlight();
}

void AvcLcd::trackHeading (int GpsHeading, int compassHeading, boolean refresh) {
  if (refresh) {
    lcd.clear();
  }
  lcd.home();
  lcd << F("GPS: ") << GpsHeading << F("  ");
  lcd.setCursor(0, 1);
  lcd << F("Compass: ") << compassHeading << F("  ");
  lcd.backlight();
}

void AvcLcd::askSetOffset(int latOffset, int lonOffset, boolean refresh) {
  if (refresh) {
    lcd.clear();
    lcd.backlight();
  }
  lcd.home();
  lcd << F("SET OFFSET?");
  lcd.setCursor(0, 1);
  lcd << F("T: ") << latOffset << F(" N: ") << lonOffset << F("   ");
}

void AvcLcd::showRunLocation(byte runLoc) {
  lcd.clear();
  lcd.home();
  lcd << F("LOCATION");
  lcd.setCursor(0, 1);
  lcd << runLocNames[runLoc - 1];
}

void AvcLcd::printHello() {
  lcd.clear();
  lcd.home();
  lcd << F("HELLO");
}
